/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-15 03:26:31
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-13 07:28:49
 * @Description:
 * @Reference:
 */
#ifndef _VILENS_PIPELINE_OFFLINE_H_
#define _VILENS_PIPELINE_OFFLINE_H_

#include "lci_slam/vilens/common_header.h"

#include "lci_slam/vilens/camera/camera_processor.h"
#include "lci_slam/vilens/imu/imu_processor.h"
#include "lci_slam/vilens/lidar/lidar_preprocessor.h"
#include "lci_slam/vilens/lidar/lidar_processor.h"
#include "lci_slam/vilens/utility/config.h"
#include "lci_slam/vilens/utility/helper.h"

namespace vilens {

class offlinePipeline {
public:
    typedef std::shared_ptr<offlinePipeline> Ptr;
    explicit offlinePipeline(const std::string &config_path);
    ~offlinePipeline() {}

    void run();
    void reset();
    void registerRosVisualizer(ros::NodeHandle &nh);

    // sensor data callback
    // Imu
    void pushData(const sensor_msgs::Imu::Ptr &msg);
    // standard_pcl
    void pushData(const sensor_msgs::PointCloud2::Ptr &msg);

    // Image
    void pushData(
        const std::vector<std::pair<int, sensor_msgs::Image::Ptr>> &msg);

    // config setting
    config::Ptr pipeline_config;

    imuMeasurements::Ptr getImuMeasurement(const double &curr_timestamp,
                                           const double &prev_timestamp = -1);

protected:
    std::shared_ptr<std::thread> imuThread, lidarThread;

    void loadParameter(const std::string &config_path);
    void allocateMemory();
    int lidarProcess();
    int imuProcess();

    std::mutex mtx_data_buffer;

    // imu
    imuProcessor::Ptr imu_process_ptr;
    boost::circular_buffer<sensor_msgs::Imu::Ptr> imu_data_buffer;
    double last_timestamp_imu = -1;
    Eigen::Vector3d accBias = Eigen::Vector3d::Zero();
    Eigen::Vector3d gyroBias = Eigen::Vector3d::Zero();
    Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81);
    bool imu_calibrated = false;
    double first_imu_time = 0.;
    double imu_calib_time = 5;

    // lidar
    lidarPreprocessor::Ptr lidar_preprocess_ptr;
    lidarProcessor::Ptr lidar_process_ptr;
    std::queue<sensor_msgs::PointCloud2::Ptr> lidar_data_buffer;
    double last_timestamp_lidar = -1;
    double delta_time_imu_lidar = 0.0;

    // img
    cameraProcessor::Ptr camera_process_ptr;
    double delta_time_img_lidar = 0.0;

    // Visualizer
    bool useViz = false;
    // Rviz publisher
    ros::Publisher pubLaserMap;
    ros::Publisher pubRawLaserScan;
};

}  // namespace vilens

#endif